#include<Servo.h>
Servo myservo[5];

#define L2 125
#define L3 125

int servo_count = 5;
int servo_pin[5]={11,10,7,6,4};

void setup() {
  delay(1500);
  Serial.begin(115200);
  for(int i=0;i<servo_count;i++){
    myservo[i].attach(servo_pin[i]);
    delay(10);
  }
  myservo[4].write(11);
  servo_move(90, 91, 108, 135);
  delay(1000);
  delay(3000);
}

void loop() {
  put_left();
  delay(2000);
  put_right();
  delay(2000);
}

void put_left(){
  servo_move(90, 91, 108, 135);   //初始化动作
  delay(1000);
  get_coordinate(150 , 0, 25);    //准备抓取物体
  delay(1000);
  myservo[4].write(90);   //爪子闭合，抓取待测物体
  delay(1000);
  servo_move(90, 91, 108, 135);   //回复到初始位置
  delay(1000);
  servo_move(180, 91, 108, 135);   //回复到初始位置
  delay(1000);
  get_coordinate(0 , -150, 35);    //准备将物体放到左边
  delay(1000);
  myservo[4].write(11);    //爪子张开，将物体放到左边
  delay(1000);
  servo_move(90, 91, 108, 135);   //初始化动作
  delay(1000);
}

void put_right(){
  servo_move(90, 91, 108, 135);   //初始化动作
  delay(1000);
  get_coordinate(150 , 0, 25);    //准备抓取物体
  delay(1000);
  myservo[4].write(90);   //爪子闭合，抓取待测物体
  delay(1000);
  servo_move(90, 91, 108, 135);   //回复到初始位置
  delay(1000);
  servo_move(0, 91, 108, 135);   //回复到初始位置
  delay(1000);
  get_coordinate(0 , 150, 35);    //准备将物体放到右边
  delay(1000);
  myservo[4].write(11);    //爪子张开，将物体放到右边
  delay(1000);
  servo_move(90, 91, 108, 135);   //初始化动作
  delay(1000);
}

void get_coordinate(float x1,float y1,float z1){
  float a, b, c, x, y, z;
  double alpha, theta, theta1, theta2 ,beta ,beta1 ,gamma;

  a = L2; 
  b = L3;

  x = x1;
  y = y1;
  z = z1;
  
  c = sqrt(x*x+y*y+z*z);
  Serial.print("c:");Serial.println(c);
  theta1 = (acos((a*a+c*c-b*b)/(2*a*c)))*(180 / PI);
  Serial.print("theta1:");Serial.println(theta1);
  theta2 = atan(z/(sqrt(x*x+y*y)))*(180 / PI);
  Serial.print("theta2:");Serial.println(theta2);  
  theta = 91 - (90 - (theta1 + theta2));
  alpha = 90 - (atan(y/x))*(180 / PI); 
  beta1 = acos((a*a+b*b-c*c)/(2*a*b))*(180 / PI);
  beta = 108 - (90 - beta1);
  gamma = 135 + (theta - beta);
  Serial.print("alpha:");Serial.println(alpha);
  Serial.print("theta:");Serial.println(theta);
  Serial.print("beta:");Serial.println(beta);
  servo_move(alpha,theta,beta,gamma);
}

void servo_move(float value0, float value1, float value2, float value3)
{
  myservo[0].writeMicroseconds(map(value0,0,270,500,2500));
  myservo[1].writeMicroseconds(map(value1,0,270,500,2500));
  myservo[2].writeMicroseconds(map(value2,0,270,500,2500));
  myservo[3].writeMicroseconds(map(value3,0,270,500,2500));
}
